/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 *
 * \file
 * \brief This is an example for using the Machine control Interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "MachineCtrlClient.hpp"
#include "gimi.h"
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//*****************************************************************************
using namespace MaCI::MachineCtrl;
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
//static gimi::GIMI g;
static volatile bool run = true;
static ownThreadHandle signalHandlerThread;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tMachineControlClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tMaCI group name\n\n");
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

static int SignalHandlerThread(void *)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(1,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
    case SIGHUP:
      dPrint(1,"Terminating...");
      run = false;
      //   g.stop();
      dPrint(1,"Waiting 3 seconds for cleanup...");
      ownSleep_ms(2000);
      dPrint(1,"Exit.");
      ownSleep_ms(1000);
      exit(1); // Force :)
      break;
      
    default:
      dPrint(1,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************

static bool DoAlarmClientCheck(TAlarmClientMap &aAlarmClients){
  
  using namespace MaCI::Alarm;
  int index = 1;
  for(EACH_IN_i(aAlarmClients)){

   dPrint(1,"Alarm client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   CAlarmData data;
   unsigned int timeout = 1000;
   dPrint(1,"Waiting for alarm event (timeout %u)",timeout);
   if(i->second->GetAlarmEvent(data,timeout)){
     dPrint(1,"GOT alarm event");
     const TAlarmEvent *ae = data.GetAlarmEvent();
     const TText *at = data.GetText();
     
     if (ae) {
       char str[80] = "-------------------------------------------------------------------------------";
       int j = sprintf(str+2," Event: type:%d, timeout:%d, flags:%d, id:%d",
                       ae->type, ae->timeout_s, ae->flags, ae->id);
       str[2+j] = ' ';
       fprintf(stdout, "%s\n", str);
     }
     if (at) {
       fprintf(stdout,
               "-------------------------------------------------------------------------------\n"
               "%s\n"
               "*******************************************************************************\n", 
                at->text);
     }
     
     dPrint(2,"Got new AlarmData Event from client");
     data.Print(3);

   }else{
     dPrint(1,"No alarm data within timeout..");
   }
   index++;
  }
   
  return true;
}
//*****************************************************************************

static bool DoBehaviourClientCheck(TBehaviourClientMap &aBehaviourClients){
  using namespace MaCI::Behaviour;
  int index = 1;
  for(EACH_IN_i(aBehaviourClients)){

   dPrint(1,"Behaviour client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   TBehaviourInformation info;
   if(i->second->GetBehaviourInfo(info,
                                  1000)){
     switch(info.type){
     case KBehaviourTypeNormal:{
       dPrint(1,"Got behaviour type = NORMAL");
       break;
     }

     case KBehaviourTypeAiding:{
       dPrint(1,"Got behaviour type = AIDING");
       break;
     }

     case KBehaviourTypeProtective:{
       dPrint(1,"Got behaviour type = PROTECTIVE");
       break;
     }
     default:{

       dPrint(1,"Unknown behaivour type = %d",info.type);
       break;
     }
     }
   }else{
     dPrint(1,"Failed to get behaviour client info");
   }

   index++;
  }

  return true;
}
//*****************************************************************************

static bool DoCoordinateDriveClientCheck(TCoordinateDriveClientMap &aCoordinateDriveClients){
  using namespace MaCI::CoordinateDrive;
  int index = 1;
  for(EACH_IN_i(aCoordinateDriveClients)){

   dPrint(1,"CoordinateDrive client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   



   index++;
  }
  return true;

}
//*****************************************************************************

static bool DoEmergencyStopClientCheck(TEmergencyStopClientMap &aEmergencyStopClients){
 using namespace MaCI::EmergencyStop;
  int index = 1;
  for(EACH_IN_i(aEmergencyStopClients)){

   dPrint(1,"EmergencyStop client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   CEmergencyStopData data;
   if(i->second->GetEmergencyStopStatusEvent(data,NULL,100)){
     dPrint(1,"Got emergency stop status event");
     const TStatus *status = data.GetStatus();

     if(status){
       if(status->emergencyStopStatus == KStatusEmergencyStopDeactivated){
         dPrint(1,"Emergency stop currently DEACTIVATED!\n");
       }else{
         dPrint(1,"Emergency stop currently ACTIVATED!\n");
       }
     }else{
       dPrint(1,"No status struct in data container ?!");
     }
   }else{
     dPrint(1,"Failed to get emergency stop status event...");
   }

   index++;
  }
 return true;
}
//*****************************************************************************

static bool DoEnergyClientCheck(TEnergyClientMap &aEnergyClients){
 using namespace MaCI::Energy;
  int index = 1;
  for(EACH_IN_i(aEnergyClients)){

   dPrint(1,"Energy client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   CEnergyData dd;
   bool r;
   unsigned int sequence = 0;
   r = i->second->GetEnergyEvent(dd, &sequence, 5000);
   if (r) {
     dPrint(1,"Got Energy event:");
     dd.Print(1);
   }else{
     dPrint(1,"Failed to get energy event!");
   }
   

   index++;
  }
 return true;
}
//*****************************************************************************

static bool DoImageClientCheck(TImageClientMap &aImageClients){
 using namespace MaCI::Image;
  int index = 1;
  for(EACH_IN_i(aImageClients)){

   dPrint(1,"Image client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   CImageData id;
   CImageContainer sid;
   
   dPrint(1,"Reading SourceInfoMap...");
   TSourceInfoMap sim;
   if (i->second->GetSourceInfoMap(sim)) {
     dPrint(1,"Got %u entries in SourceInfoMap", sim.size());
     for(EACH_IN_j(sim)) {
       dPrint(1,"%u: '%s' - '%s'",
              j->second.sourceindex, 
              j->second.sourcename, 
              j->second.sourcedescription);
     }
     
   } else {
     dPrint(1, "WARNING: Failed to read SourceInfoMap from source!");
     
   }
   

   index++;
  }
 return true;
}
//*****************************************************************************

static bool DoIMUClientCheck(TIMUClientMap &aIMUClients){
 using namespace MaCI::IMU;
  int index = 1;
  for(EACH_IN_i(aIMUClients)){

   dPrint(1,"IMU client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   
   MaCI::IMU::TAcceleration3d acc;
   MaCI::Common::TTimestamp tstamp;
   dPrint(1,"Trying to get accelerations..");
   if(i->second->GetAccelerations(acc,tstamp,NULL,1000)){
     dPrint(1,"Got accelerations(timestamp %u %u): x %f, y %f, z %f",
             tstamp.timestamp_s,
             tstamp.timestamp_us,
             acc.accelerationX,
             acc.accelerationY,
             acc.accelerationZ);
   }else{
     dPrint(1,"failed to get accelerations!");
   }
   
   
   
   index++;
  }
  return true;
}
//*****************************************************************************

static bool DoIOClientCheck(TIOClientMap &aIOClients){
 using namespace MaCI::IO;
  int index = 1;
  for(EACH_IN_i(aIOClients)){

   dPrint(1,"IO client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   




   index++;
  }
 return true;
}
//*****************************************************************************

static bool DoJointGroupCtrlClientCheck(TJointGroupCtrlClientMap &aJointGroupCtrlClients){
  using namespace MaCI::JointGroupCtrl;
  int index = 1;
  for(EACH_IN_i(aJointGroupCtrlClients)){

   dPrint(1,"JointGroupCtrl client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   
   TJointGroupInformation info;
   dPrint(1,"Getting joint Group information...");
   if(i->second->GetJointGroupInformation(info,1000)){

     int numberOfJoints = info.size();
     for(int j = 0; j< numberOfJoints; j++){
       dPrint(1,"%d. name: %s, number %d",j,
              info[j].jointName.c_str(),
              info[j].jointNumber);
       if(info[j].jointControlType == MaCI::JointGroupCtrl::KJointControlAngular){
         dPrint(1," It is an angular joint");
       }else if(info[j].jointControlType == MaCI::JointGroupCtrl::KJointControlLinear){
         dPrint(1," It is a linear joitn");
       }

       if(info[j].canControlSpeed){
         dPrint(1,"Can be controlled by speed commands, maxSpeed %f",info[j].maxSpeed);
       }
       if(info[j].canControlPosition){
         dPrint(1,"Can be controlled by position commands, minPosition %f, maxPosition %f",
                info[j].minPosition,
                info[j].maxPosition);
       }
       if(info[j].canControlForce){
         dPrint(1,"Can be controlled by force commands, minForce %f, maxForce %f",
                info[j].minForce,
                info[j].maxForce);
       }

          
     }
     
   }else{
     dPrint(1,"FAILED to get joint group information!");
   }



   index++;
  }
 return true;
}
//*****************************************************************************

static bool DoMapClientCheck(TMapClientMap &aMapClients){
 using namespace MaCI::Map;
  int index = 1;
  for(EACH_IN_i(aMapClients)){

   dPrint(1,"Map client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   



   index++;
  }
 return true;
}
//*****************************************************************************

static bool DoPositionClientsCheck( TPositionClientMap &aPositionClients){

   
  TPositionClientMap::iterator iterpos = aPositionClients.begin();
  int index = 1;
  while(iterpos != aPositionClients.end()){
    dPrint(1,"Position client n.%d. Instance name '%s' and pointer value %p",index,iterpos->first.c_str(),iterpos->second);
    
    
    dPrint(1,"Asking current pose:");
    MaCI::Position::CPositionData posdata;
    int seq = -1;
    iterpos->second->GetPositionEvent(posdata, &seq,1000);
    const MaCI::Position::TPose2D *pose = posdata.GetPose2D();
    if(pose){
      dPrint(1,"Current pose is %f,%f angle: %f",pose->x,pose->y, pose->a);
    }else{
      dPrint(1,"Did not get current pose :(");
    }

    iterpos++;
    index++;
  }

  dPrint(1, "Position clients succesfully tested");
  return true;

}
//*****************************************************************************

static bool DoRangingClientsCheck(TRangingClientMap &aRangingClients){

  TRangingClientMap::iterator iter = aRangingClients.begin();

  int clientNumber = 1;
  bool gr;
  while(iter != aRangingClients.end()){
    dPrint(1,"Ranging Client n.%d. Instance name '%s' and pointer value %p",clientNumber,iter->first.c_str(),iter->second);
    
    int index = -1;
    MaCI::Common::TTimestamp timestamp;
    MaCI::Ranging::TDistanceHeader header;
    MaCI::Ranging::TDistanceArray array;
    MaCI::Ranging::TDeviceInformationLimits limits;
    MaCI::Ranging::TDeviceInformationPosition pose;
    gr = iter->second->GetDevicePosition(pose, 1000, false);
    if(gr){
      dPrint(1,"Got Ranging Device position: (%.2f, %.2f, %.2f) pointing at %.3f rad", 
             pose.x, pose.y, pose.z, pose.heading);
    }else{
      dPrint(1,"Failed to get device position!");
    }

    gr = iter->second->GetDeviceLimits(limits, 1000, false);

    if(gr){
      dPrint(1,"Got Ranging device limits: Min:%.3f max:%.3f", 
             limits.min_distance, limits.max_distance);
    }else{
      dPrint(1,"Failed to get device limits!");
    }
   
    gr = iter->second->GetDistanceArray(array, &header, &timestamp, &index, 1000);
    if(gr){
      dPrint(1,"Got distance array... (Contains %d Distances)", header.beam_count);

    }else{
      dPrint(1,"Failed to get distance array!");
    }
      clientNumber++;
    iter++;
  }

  return true;
}
//*****************************************************************************

static bool DoSpeedCtrlClientCheck(TSpeedCtrlClientMap &aSpeedCtrlClients){
 using namespace MaCI::SpeedCtrl;
  int index = 1;
  for(EACH_IN_i(aSpeedCtrlClients)){

   dPrint(1,"SpeedCtrl client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   i->second->StartGettingSpeedEvent();



   index++;
  }
 return true;
}
//*****************************************************************************
 
static bool DoTextClientCheck(TTextClientMap &aTextClients){
 using namespace MaCI::Text;
  int index = 1;
  for(EACH_IN_i(aTextClients)){

   dPrint(1,"Text client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   



   index++;
  }
 return true;
}
//*****************************************************************************
 
static bool DoUnitClientCheck(TUnitClientMap &aUnitClients){
 using namespace MaCI::Unit;
  int index = 1;
  for(EACH_IN_i(aUnitClients)){

   dPrint(1,"Unit client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   



   index++;
  }
 return true;
}
//*****************************************************************************
 
static bool DoWirelessClientCheck(TWirelessClientMap &aWirelessClients){
 using namespace MaCI::Wireless;
  int index = 1;
  for(EACH_IN_i(aWirelessClients)){

   dPrint(1,"Wireless client n.%d. Instance name '%s' and pointer value %p",index,i->first.c_str(),i->second);
   



   index++;
  }
 return true;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
int main(int argc, char *argv[]) 
{
    // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(2);
  // GIMnet parameters
  std::string gimnetAP = "localhost";
  int gimnetAPPort = 40002;
  std::string gimnetName = "";
  std::string gimnetSourceName = "";
  std::string datasourceMaCISL;
  std::string maciGroup ="FSRSim.J2B2";
 

 // InitSignalHandler
  InitSignalHandler();

  // Greet
  fprintf(stderr,
	  "\nMachienCtrlClientExample - MaCI MachineCtrlClient Interface Example - v1.0 Beta\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "G:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
   
    case 'G':
      maciGroup = optarg;
      dPrint(3,"Using Group '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }


  dPrint(1,"Constructing MachineCtrlClient...");
  CMachineCtrlClient machine(gimnetAP,gimnetAPPort,"",maciGroup);
  TMaCIMachineComponents machineComponents;
  machine.GetMachineComponents(machineComponents);
  dPrint(1,"Machine '%s' consists of %d clients:",maciGroup.c_str(),machineComponents.size());
  for(EACH_IN_i(machineComponents)){
    dPrint(1,"interface type %d, instance name: %s",i->first,i->second.c_str());

  }


  
  /*
  //Checking for Alarm interfaces
  TAlarmClientMap alarmClients = machine.GetAlarmClients();
  dPrint(1,"************DOING ALARM CLIENT TESTS*****************");
  if(alarmClients.size() > 0){
    dPrint(1,"Machine contains %d alarm clients. Opening them all", alarmClients.size());
    machine.OpenClients(KTypeAlarm);
    DoAlarmClientCheck(alarmClients);
  }else{
    dPrint(1,"NO ALARM CLIENT FOUND. Skipping alarm client tests");
  }


  //Checking for Behaviour interfaces
  TBehaviourClientMap behaviourClients = machine.GetBehaviourClients();
  dPrint(1,"************DOING BEHAVIOUR CLIENT TESTS*****************");
  if(behaviourClients.size() > 0){
    dPrint(1,"Machine contains %d behaviour clients. Opening them all", behaviourClients.size());
    machine.OpenClients(KTypeBehaviour);
    DoBehaviourClientCheck(behaviourClients);
  }else{
    dPrint(1,"NO BEHAVIOUR CLIENT FOUND. Skipping behaviour client tests");
  }


  //Checking for CoordinateDrive interfaces
  dPrint(1,"************DOING COORDINATEDRIVE CLIENT TESTS*****************");
  TCoordinateDriveClientMap coordinatedriveClients = machine.GetCoordinateDriveClients();
  if(coordinatedriveClients.size() > 0){
    dPrint(1,"Machine contains %d coordinatedrive clients. Opening them all", coordinatedriveClients.size());
    machine.OpenClients(KTypeCoordinateDrive);
    DoCoordinateDriveClientCheck(coordinatedriveClients);
  }else{
    dPrint(1,"NO COORDINATEDRIVE CLIENT FOUND. Skipping coordinateDrive client tests");
  }

  //Checking for Emergencystop interfaces
    dPrint(1,"************DOING EMERGENCYSTOP CLIENT TESTS*****************");
  TEmergencyStopClientMap emergencystopClients = machine.GetEmergencyStopClients();
  if(emergencystopClients.size() > 0){
    dPrint(1,"Machine contains %d emergencystop clients. Opening them all", emergencystopClients.size());
    machine.OpenClients(KTypeEmergencyStop);
    DoEmergencyStopClientCheck(emergencystopClients);
  }else{
    dPrint(1,"NO EMERGENCYSTOP CLIENT FOUND. Skipping emergencystop client tests");
  }


  //Checking for Energy interfaces
  dPrint(1,"************DOING ENERGY CLIENT TESTS*****************");
  TEnergyClientMap energyClients = machine.GetEnergyClients();
  if(energyClients.size() > 0){
    dPrint(1,"Machine contains %d energy clients. Opening them all", energyClients.size());
    machine.OpenClients(KTypeEnergy);
    DoEnergyClientCheck(energyClients);
  }else{
    dPrint(1,"NO ENERGY CLIENT FOUND. Skipping energy client tests");
  }


  //Checking for Image interfaces
  dPrint(1,"************DOING IMAGE CLIENT TESTS*****************");
  TImageClientMap imageClients = machine.GetImageClients();
  if(imageClients.size() > 0){
    dPrint(1,"Machine contains %d image clients. Opening them all", imageClients.size());
    machine.OpenClients(KTypeImage);
    DoImageClientCheck(imageClients);
  }else{
    dPrint(1,"NO IMAGE CLIENT FOUND. Skipping image client tests");
  }


 //Checking for IMU interfaces
  dPrint(1,"************DOING IMU CLIENT TESTS*****************");
  TIMUClientMap IMUClients = machine.GetIMUClients();
  if(IMUClients.size() > 0){
    dPrint(1,"Machine contains %d IMU clients. Opening them all", IMUClients.size());
    machine.OpenClients(KTypeIMU);
    DoIMUClientCheck(IMUClients);
  }else{
    dPrint(1,"NO IMU CLIENT FOUND. Skipping IMU client tests");
  }


 //Checking for IO interfaces
  dPrint(1,"************DOING IO CLIENT TESTS*****************");
  TIOClientMap IOClients = machine.GetIOClients();
  if(IOClients.size() > 0){
    dPrint(1,"Machine contains %d IO clients. Opening them all", IOClients.size());
    machine.OpenClients(KTypeIO);
    DoIOClientCheck(IOClients);
  }else{
    dPrint(1,"NO IO CLIENT FOUND. Skipping IO client tests");
  }


 //Checking for JointGroupCtrl interfaces
  dPrint(1,"************DOING JOINTGROUPCTRL CLIENT TESTS*****************");
  TJointGroupCtrlClientMap jointGroupCtrlClients = machine.GetJointGroupCtrlClients();
  if(jointGroupCtrlClients.size() > 0){
    dPrint(1,"Machine contains %d jointGroupCtrl clients. Opening them all", jointGroupCtrlClients.size());
    machine.OpenClients(KTypeJointGroupCtrl);
    DoJointGroupCtrlClientCheck(jointGroupCtrlClients);
  }else{
    dPrint(1,"NO JOINTGROUPCTRL CLIENT FOUND. Skipping jointGroupCtrl client tests");
  }

 //Checking for Map interfaces
  dPrint(1,"************DOING MAP CLIENT TESTS*****************");
  TMapClientMap mapClients = machine.GetMapClients();
  if(mapClients.size() > 0){
    dPrint(1,"Machine contains %d map clients. Opening them all", mapClients.size());
    machine.OpenClients(KTypeMap);
    DoMapClientCheck(mapClients);
  }else{
    dPrint(1,"NO MAP CLIENT FOUND. Skipping map client tests");
  }


  //Checkting for Position interfaces 
  dPrint(1,"************DOING POSITION CLIENT TESTS*****************");
  TPositionClientMap posClients = machine.GetPositionClients();
  if(posClients.size() > 0){

    dPrint(1,"Machine contains %d position clients. Opening them all",posClients.size());
    machine.OpenClients(KTypePosition);
    DoPositionClientsCheck(posClients);
  }else{
    dPrint(1,"NO POSITION CLIENTS FOUND, Skipping position client tests");
  }

  //Checkting for Ranging interfaces 
  dPrint(1,"************DOING RANGING CLIENT TESTS*****************");
  TRangingClientMap rangingClients = machine.GetRangingClients();
  if(rangingClients.size() > 0){

    dPrint(1,"Machine contains %d ranging clients. Opening them all", rangingClients.size());
    machine.OpenClients(KTypeRanging);
    DoRangingClientsCheck(rangingClients);
  }else{
    dPrint(1,"NO RANGING CLIENTS FOUND, Skipping position client tests");
  }
  
  
 //Checking for SpeedCtrl interfaces
  dPrint(1,"************DOING SPEEDCTRL CLIENT TESTS*****************");
  TSpeedCtrlClientMap speedCtrlClients = machine.GetSpeedCtrlClients();
  if(speedCtrlClients.size() > 0){

    dPrint(1,"Machine contains %d speedCtrl clients. Opening them all", speedCtrlClients.size());
    machine.OpenClients(KTypeSpeedCtrl);
    DoSpeedCtrlClientCheck(speedCtrlClients);
  }else{
    dPrint(1,"NO SPEEDCTRL CLIENT FOUND. Skipping speedCtrl client tests");
  }
  //Checking for Text interfaces
  dPrint(1,"************DOING TEXT CLIENT TESTS*****************");
  TTextClientMap textClients = machine.GetTextClients();
  if(textClients.size() > 0){

    dPrint(1,"Machine contains %d text clients. Opening them all", textClients.size());
    machine.OpenClients(KTypeText);
    DoTextClientCheck(textClients);
  }else{
    dPrint(1,"NO TEXT CLIENT FOUND. Skipping text client tests");
  }

 //Checking for Unit interfaces
  dPrint(1,"************DOING UNIT CLIENT TESTS*****************");
  TUnitClientMap unitClients = machine.GetUnitClients();
  if(unitClients.size() > 0){

    dPrint(1,"Machine contains %d unit clients. Opening them all", unitClients.size());
    machine.OpenClients(KTypeUnit);
    DoUnitClientCheck(unitClients);
  }else{
    dPrint(1,"NO UNIT CLIENT FOUND. Skipping unit client tests");
  }

 //Checking for Wireless interfaces
  dPrint(1,"************DOING WIRELESS CLIENT TESTS*****************");
  TWirelessClientMap wirelessClients = machine.GetWirelessClients();
  if(wirelessClients.size() > 0){
    dPrint(1,"Machine contains %d wireless clients. Opening them all", wirelessClients.size());
    machine.OpenClients(KTypeWireless);
    DoWirelessClientCheck(wirelessClients);
  }else{
    dPrint(1,"NO WIRELESS CLIENT FOUND. Skipping wireless client tests");
  }
  */

  dPrint(1,"***************ALL TEST PERFORMED*********************");
  dPrint(1,"Closing all clients");
  machine.CloseClients(KTypeAll);


  dPrint(1,"Bye Bye");

  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

